#include "SurfIcpMatcherI.h"
#include <vector>

using namespace std;

SurfIcpMatcherI::SurfIcpMatcherI()
{
	name = "SurfIcpMatcherI";
	printf("new SurfIcpMatcherI\n");
	nr_iter = 10;
	distance_scale = 0.05f;
	distance_threshold = 0.05f * distance_scale;
	surf_threshold = 0.2f;
}

SurfIcpMatcherI::~SurfIcpMatcherI()
{
	printf("delete SurfIcpMatcherI\n");
}

void SurfIcpMatcherI::update(){
}

const bool debugg_SurfIcpMatcherI = false;
Transformation * SurfIcpMatcherI::getTransformation(FrameNode * src, FrameNode * dst)
{
	//pcl::visualization::CloudViewer viewer("Cloud Viewer");
	if(debugg_SurfIcpMatcherI){printf("SurfIcpMatcherI::getTransformation(%i,%i)\n",src->frame->id_number,dst->frame->id_number);}

	IplImage* img_combine;
	int sx;
	int sy;
	if(debugg_SurfIcpMatcherI)
	{
		IplImage* img_a = cvLoadImage( src->frame->rgb_path.c_str(), 1);
		IplImage* img_b = cvLoadImage( dst->frame->rgb_path.c_str(), 1);
	
		sx = img_a->width;
		sy = img_a->height;
		
		img_combine = cvCreateImage(cvSize(2*sx,sy), IPL_DEPTH_8U, 3);
		for (int i = 0; i < sx; i++)
		{
			for (int j = 0; j < sy; j++)
			{
				img_combine->imageData[3 * (j * (2*sx) + (sx+i)) + 0] = img_a->imageData[3 * (j * sx + i) + 0];
				img_combine->imageData[3 * (j * (2*sx) + (sx+i)) + 1] = img_a->imageData[3 * (j * sx + i) + 1];
				img_combine->imageData[3 * (j * (2*sx) + (sx+i)) + 2] = img_a->imageData[3 * (j * sx + i) + 2];
			}
		}
	
		for (int i = 0; i < sx; i++)
		{
			for (int j = 0; j < sy; j++)
			{
				img_combine->imageData[3 * (j * (2*sx) + (i)) + 0] = img_b->imageData[3 * (j * sx + i) + 0];
				img_combine->imageData[3 * (j * (2*sx) + (i)) + 1] = img_b->imageData[3 * (j * sx + i) + 1];
				img_combine->imageData[3 * (j * (2*sx) + (i)) + 2] = img_b->imageData[3 * (j * sx + i) + 2];
			}
		}
	}
		
	KeyPointSet * src_keypointset 	= src->frame->keypoints;
	KeyPointSet * dst_keypointset 	= dst->frame->keypoints;
	KeyPoint ** src_keypoints	 	= src->frame->keypoints->valid_key_points;
	KeyPoint ** dst_keypoints 		= dst->frame->keypoints->valid_key_points;

	int src_nr_points = src_keypointset->nr_valid_points;
	int dst_nr_points = dst_keypointset->nr_valid_points;

	float ** surf_distances = new float*[src_nr_points];
	float ** match_distances = new float*[src_nr_points];
	for(int i = 0; i < src_nr_points;i++)
	{
		surf_distances[i] = new float[dst_nr_points];
		match_distances[i]= new float[dst_nr_points];		
		for(int j = 0; j < dst_nr_points;j++)
		{
			FeatureDescriptor * descriptorA = src_keypoints[i]->surf_descriptor;
			FeatureDescriptor * descriptorB = dst_keypoints[j]->surf_descriptor;
			surf_distances[i][j] = descriptorA->distance(descriptorB);
		}
	}
	
	float * pos_src_x 				= new float[src_nr_points];
	float * pos_src_y 				= new float[src_nr_points];
	float * pos_src_z 				= new float[src_nr_points];
	float * pos_src_x_transform 	= new float[src_nr_points];
	float * pos_src_y_transform 	= new float[src_nr_points];
	float * pos_src_z_transform 	= new float[src_nr_points];
	
	float * pos_dst_x 				= new float[dst_nr_points];
	float * pos_dst_y 				= new float[dst_nr_points];
	float * pos_dst_z 				= new float[dst_nr_points];
	float * pos_dst_x_transform 	= new float[dst_nr_points];
	float * pos_dst_y_transform 	= new float[dst_nr_points];
	float * pos_dst_z_transform 	= new float[dst_nr_points];
	
	int * src_matches 				= new int[src_nr_points];
	int * dst_matches 				= new int[dst_nr_points];
	
	KeyPoint ** src_match_id 		= new KeyPoint*[src_nr_points];
	KeyPoint ** dst_match_id 		= new KeyPoint*[src_nr_points];
		
	int * src_matches_id 			= new int[src_nr_points];
	int * dst_matches_id 			= new int[src_nr_points];
	
	for(int i = 0; i < src_nr_points;i++)
	{
		pos_src_x[i] = src_keypoints[i]->position(0);
		pos_src_y[i] = src_keypoints[i]->position(1);
		pos_src_z[i] = src_keypoints[i]->position(2);
		if(debugg_SurfIcpMatcherI){cvCircle(img_combine,cvPoint(src_keypoints[i]->frame_x, src_keypoints[i]->frame_y), 5,cvScalar(0, 0, 255, 0),2, 8, 0);}
	}
	for(int i = 0; i < dst_nr_points;i++)
	{
		pos_dst_x[i] = dst_keypoints[i]->position(0);
		pos_dst_y[i] = dst_keypoints[i]->position(1);
		pos_dst_z[i] = dst_keypoints[i]->position(2);
		if(debugg_SurfIcpMatcherI){cvCircle(img_combine,cvPoint(dst_keypoints[i]->frame_x + sx, dst_keypoints[i]->frame_y), 5,cvScalar(0, 0, 255, 0),2, 8, 0);}
	}
	
	Eigen::Matrix4f transformationMat = Eigen::Matrix4f::Identity();
	Transformation * transformation = new Transformation();
	transformation->transformationMatrix = transformationMat;
	transformation->src = src;
	transformation->dst = dst;
	transformation->weight = 100;
	transformation->level = 1;
	transformation->matches_src = new KeyPoint*[src_nr_points];
	transformation->matches_dst = new KeyPoint*[src_nr_points];
	transformation->nr_matches  = 0;
	transformation->next = 0;
	transformation->prev = 0;
	Transformation * first_trans = transformation; 
	
	float step_size = 1.0f/(float)nr_iter;
	for(float alpha = 0; alpha <= 1.0; alpha+=step_size)
	{
		float mat00 = transformationMat(0,0);
		float mat01 = transformationMat(0,1);
		float mat02 = transformationMat(0,2);
		float mat03 = transformationMat(0,3);
		float mat10 = transformationMat(1,0);
		float mat11 = transformationMat(1,1);
		float mat12 = transformationMat(1,2);
		float mat13 = transformationMat(1,3);
		float mat20 = transformationMat(2,0);
		float mat21 = transformationMat(2,1);
		float mat22 = transformationMat(2,2);
		float mat23 = transformationMat(2,3);
		float mat30 = transformationMat(3,0);
		float mat31 = transformationMat(3,1);
		float mat32 = transformationMat(3,2);
		float mat33 = transformationMat(3,3);
		/*
		printf("transformationMat:\n");
		for(int i = 0; i < 4; i++)
		{
			for(int j = 0; j < 4; j++)
			{
				printf("%f, ", transformationMat(i,j));
			}
			printf("\n");
		}
		*/
		for(int i = 0; i < src_nr_points;i++)
		{
			float x = pos_src_x[i];
			float y = pos_src_y[i];
			float z = pos_src_z[i];
				
			pos_src_x_transform[i] = x*mat00+y*mat01+z*mat02+mat03;
			pos_src_y_transform[i] = x*mat10+y*mat11+z*mat12+mat13;
			pos_src_z_transform[i] = x*mat20+y*mat21+z*mat22+mat23;
		}
		for(int i = 0; i < dst_nr_points;i++)
		{
			float x = pos_dst_x[i];
			float y = pos_dst_y[i];
			float z = pos_dst_z[i];
				
			pos_dst_x_transform[i] = x;//x*mat00+y*mat01+z*mat02+mat03;
			pos_dst_y_transform[i] = y;//x*mat10+y*mat11+z*mat12+mat13;
			pos_dst_z_transform[i] = z;//x*mat20+y*mat21+z*mat22+mat23;
		}
		for(int i = 0; i < src_nr_points;i++)
		{
			float x = pos_src_x_transform[i];
			float y = pos_src_y_transform[i];
			float z = pos_src_z_transform[i];
			float dx,dy,dz;
			for(int j = 0; j < dst_nr_points;j++)
			{
				dx = x-pos_dst_x_transform[j];
				dy = y-pos_dst_y_transform[j];
				dz = z-pos_dst_z_transform[j];
				//float dist = dx*dx + dy*dy + dz*dz;
				//printf("dist: %i : %i --> %f\n",i,j,dist);
				match_distances[i][j] = (1-alpha)*surf_distances[i][j] + distance_scale*alpha*(dx*dx + dy*dy + dz*dz);
			}
		}
		
		float dxx = pos_src_x_transform[0]-pos_dst_x_transform[0];
		float dyy = pos_src_y_transform[0]-pos_dst_y_transform[0];
		float dzz = pos_src_z_transform[0]-pos_dst_z_transform[0];
		float dist = dxx*dxx + dyy*dyy + dzz*dzz;
		//printf("dist[0][0] = %f\n surf[0][0] = %f\n match_distances[0][0] = %f\n",dist,surf_distances[0][0],match_distances[0][0]);
		
		
		for(int i = 0; i < src_nr_points;i++)
		{
			src_matches[i] = -1;
			float best_value = 9999999;
			for(int j = 0; j < dst_nr_points;j++)
			{
				if(match_distances[i][j]<best_value)
				{
					best_value = match_distances[i][j];
					src_matches[i] = j;
				}
			}
		}
		for(int j = 0; j < dst_nr_points;j++)
		{
			dst_matches[j] = -1;
			float best_value = 9999999;
			for(int i = 0; i < src_nr_points;i++)
			{
				if(match_distances[i][j]<best_value)
				{
					best_value = match_distances[i][j];
					dst_matches[j] = i;
				}
			}
		}
		Transformation * transformation2;
		if(debugg_SurfIcpMatcherI){
			transformation2 = new Transformation();
			transformation2->src = src;
			transformation2->dst = dst;
			transformation2->weight = alpha;
			transformation2->level = 1;
			transformation2->matches_src = new KeyPoint*[src_nr_points];
			transformation2->matches_dst = new KeyPoint*[src_nr_points];
			transformation2->nr_matches  = 0;
		}
		
		pcl::TransformationFromCorrespondences tfc;
		int nr_matches = 0;
		float threshold = distance_threshold*alpha + (1 - alpha)*surf_threshold;
		//printf("-------------------------------------------------------\n");
		for(int i = 0; i < src_nr_points;i++)
		{
			if(i == dst_matches[src_matches[i]] && match_distances[i][src_matches[i]] < threshold){
				/*
				float dxx = pos_src_x_transform[i]-pos_dst_x_transform[src_matches[i]];
				float dyy = pos_src_y_transform[i]-pos_dst_y_transform[src_matches[i]];
				float dzz = pos_src_z_transform[i]-pos_dst_z_transform[src_matches[i]];
				float dist = dxx*dxx + dyy*dyy + dzz*dzz;
				printf("dist:%i->%i = %f\n",i,src_matches[i],dist);
				*/
				src_match_id[nr_matches] = src->frame->keypoints->valid_key_points[i];
				dst_match_id[nr_matches] = dst->frame->keypoints->valid_key_points[src_matches[i]];
				tfc.add(src_match_id[nr_matches]->position, dst_match_id[nr_matches]->position);
				if(debugg_SurfIcpMatcherI){
					transformation2->matches_src[nr_matches] = src_match_id[nr_matches];
					transformation2->matches_dst[nr_matches] = dst_match_id[nr_matches];
					transformation2->nr_matches = nr_matches+1;
				}else{
					transformation->matches_src[nr_matches] = src_match_id[nr_matches];
					transformation->matches_dst[nr_matches] = dst_match_id[nr_matches];
					transformation->nr_matches = nr_matches+1;
				}
				nr_matches++;
			}
		}		
		if(debugg_SurfIcpMatcherI){
			transformation2->transformationMatrix = tfc.getTransformation().matrix();
			transformation->next = transformation2;
			transformation2->prev = transformation;
			transformation = transformation2;
			transformationMat = transformation2->transformationMatrix;
		}else
		{
			transformationMat = tfc.getTransformation().matrix();
			transformation->transformationMatrix = transformationMat;
		}
		if(debugg_SurfIcpMatcherI)
		{
			cvNamedWindow("Combination", CV_WINDOW_AUTOSIZE );
			cvShowImage("Combination", img_combine);
    		cvWaitKey(0);
    		cvReleaseImage( &img_combine );
    	}
	}
	return first_trans;
}
